#include <motor/motor.h>

#include "math.h"

#include <std_msgs/Int32.h>
#include <string.h>
#include <algorithm>

Motor::Motor() :
	simulation_(false),
	rotating_(true),
	rotate_only_(true),
	wait_until_completion_(false),
	
	current_angle_(0),
	current_x_(0),
	current_y_(0),
	current_speed_(0),

	target_angle_(0),
	target_x_(0),
	target_y_(0),
	
	prev_timestamp_(0),
	prev_left_(0),
	prev_right_(0)
{
	ros::NodeHandle n("motor");
	
	int encoder_rate;

	if (!(n.getParam("simulation", simulation_) &&
		n.getParam("encoder_ticks", encoder_ticks_) &&
		n.getParam("min_speed", min_speed_) &&
		n.getParam("max_speed", max_speed_) &&
		n.getParam("max_acceleration", max_acceleration_) &&
		n.getParam("wheel_base", wheel_base_) &&
		n.getParam("wheel_radius", wheel_radius_) &&
		n.getParam("encoder_rate", encoder_rate) &&
		n.getParam("rotation_accuracy", rotation_accuracy_) &&
		n.getParam("distance_accuracy", distance_accuracy_))) {
		ROS_ERROR("driver/motor: Could not read parameter from parameter server!");
	}

	float wheel_circumference = wheel_radius_ * 2.0f * M_PI;
	distance_per_tick_ = wheel_circumference / encoder_ticks_;
	
	// Subscribes to the topic /encoder with the receive_encoder function as callback.
	enc_sub = n.subscribe("/serial/encoder", 1000, &Motor::receiveEncoder, this);

	// Subscribes to the topic /motor_controller/directions
	mov_sub = n.subscribe("/motor_controller/directions", 1, &Motor::receiveDirections, this);

	// Gets a Publisher used to publish to the motor_speed topic.
	mot_pub = n.advertise<robo::Motor>("/serial/motor_speed", 10);

	// Gets a Publisher used to publish to the encoder/interval topic.
	int_pub = n.advertise<std_msgs::Int32>("/serial/encoder_interval", 1);

	// Gets a Publisher used to publish to the /motor_controller/movement topic.
	mov_pub = n.advertise<driver::Movement>("/motor_controller/movement", 1000);

	// Create Rate object
	ros::Rate loop_rate(encoder_rate);
	
	// Sync the loop_rate to the encoder interval.
	std_msgs::Int32 interval_msg;
	interval_msg.data = 1000 / encoder_rate;

	// Wait for subscriber before sending desired interval.
	while (int_pub.getNumSubscribers() == 0) {
		ros::spinOnce();
		loop_rate.sleep();
	}

	// Send the desired interval to interval topic.
	int_pub.publish(interval_msg);
	
	while(ros::ok()){
		// Allow callbacks to be called
		ros::spinOnce();

		// Limit loop rate by calling sleep()
		loop_rate.sleep();
	}
}

void Motor::receiveDirections(const driver::Move::ConstPtr &msg) {
	if (!wait_until_completion_) {
		wait_until_completion_ = msg->wait_until_completion;
		target_angle_ = current_angle_ + msg->angle;

		target_x_ = current_x_ + msg->distance * cosf(target_angle_);
		target_y_ = current_y_ + msg->distance * sinf(target_angle_);

		// Start rotating if off by too much
//		rotating_ = msg->distance <= distance_accuracy_ && std::abs(current_angle_ - target_angle_) > rotation_accuracy_;
//		rotate_only_ = rotating_ && msg->distance <= distance_accuracy_;

		rotating_ = fabs(current_angle_ - target_angle_) > rotation_accuracy_;
		rotate_only_ = rotating_ && msg->distance <= distance_accuracy_;

		last_message_timestamp_ = prev_timestamp_;

		if (fabsf(msg->distance) < 0.001f && fabsf(msg->angle) < 0.001f) {
			last_message_timestamp_ = prev_timestamp_ - 100.0f;
		}
	}
//	// If a standstill rotation, calculate the start and end value of encoders
//	if (rotate_only_) {
//		rotate_start_left_ticks_ = prev_left_;
//		rotate_start_right_ticks_ = prev_right_;
//
//		// Length of the arc each wheel will travel
//		float arc_length = wheel_base_ * M_PI * msg->angle / M_PI;
//		float sign = (msg->angle >= 0.0f ? 1.0f : -1.0f);
//		int num_ticks = arc_length / distance_per_tick_;
//
//		rotate_end_left_ticks_ = rotate_start_left_ticks_ + sign * num_ticks;
//		rotate_end_right_ticks_ = rotate_start_right_ticks_ - sign * num_ticks;
//
//		rotate_start_angle_ = current_angle_;
//	}
}

void Motor::receiveEncoder(const robo::Encoder::ConstPtr &msg)
{
	// Exit if first odometry update since launch
	if (prev_timestamp_ == 0.0f) {
		prev_timestamp_ = msg->timestamp;
		prev_left_ = msg->left;
		prev_right_ = msg->right;
		return;
	}

	// Calculate diffs
	float diff_time = msg->timestamp - prev_timestamp_;
	int diff_left_ticks = msg->left - prev_left_;
	int diff_right_ticks = msg->right - prev_right_;

	prev_timestamp_ = msg->timestamp;
	prev_left_ = msg->left;
	prev_right_ = msg->right;

	// Update the odometry
	updateOdometry(diff_time, diff_left_ticks, (simulation_ ? diff_right_ticks : -diff_right_ticks));

	robo::Motor motor;
//	ROS_INFO("ticks: (%lld %lld)", msg->left, msg->right);
//	motor.left = 0.0;
//	motor.right = 0.0;
//	mot_pub.publish(motor);
//	return;

	// Send 0 speed if we haven't received any directions for a while.
	if (msg->timestamp - last_message_timestamp_ > 10.0f) {
		wait_until_completion_   = false;
		motor.left = motor.right = 0;
		mot_pub.publish(motor);

		// EXIT call
		return;
	}

//	if (rotate_only_) {
//		int tick_diff_left = prev_left_ - rotate_start_left_ticks_;
//		int tick_diff_right = prev_right_ - rotate_start_right_ticks_;
//
//
//
//		mot_pub.publish(motor);
//		return;
//	}

	// Calculate distance to target
	float dx = target_x_ - current_x_;
	float dy = target_y_ - current_y_;
	float target_distance = sqrtf(dx * dx + dy * dy);

	float heading_adjustment = 0.0f;
	float heading_sign = 1.0f;

	if (rotating_) {
		heading_adjustment = std::abs(target_angle_ - current_angle_);
		heading_sign = target_angle_ < current_angle_ ? 1.0f : -1.0f;
	} else {
		// Calculate needed heading adjustment by taking the dot product between
		// the current heading vector and the (current position -> target position) vector
		if (target_distance != 0.0f) {
			float cx = cosf(current_angle_);
			float cy = sinf(current_angle_);
			float dot = dx * cx + dy * cy;

			float normalized = dot / target_distance;
			normalized = normalized > 1.0f ? 1.0f : normalized;
			normalized = normalized < -1.0f ? -1.0f : normalized;

			heading_adjustment = acosf(normalized);

			// Since dot product is always positive, use the cross product to determine
			// the sign of the angle
			heading_sign = 1.0f;
			if ((dx * cy - dy * cx) < 0) {
				heading_sign = -1.0f;
			}
		}
	}

//	ROS_INFO("Position: (%f, %f, %f)", current_x_, current_y_, current_angle_);
//	ROS_INFO("Distance to target: %f", target_distance);
//	ROS_INFO("Heading off by %f degrees", heading_sign * heading_adjustment * 57.295f);

	// Scale the heading factor to the interval [0.0, 1.0]
	float heading_factor = heading_adjustment / M_PI;
	// Slow down 10cm before target
	float distance_factor = std::min(0.1f, target_distance) * (1.0f / 0.1f);
	
	// Use an exponential function to minimize jitter
	// in the course correcting
	float hf = std::pow(heading_factor, 1.0f / 3.0f);
	float df = std::pow(distance_factor, 1.0f / 3.0f);

	if (rotating_) {
		if (heading_adjustment < rotation_accuracy_) {
			motor.left = motor.right = 0.0f;
			rotating_ = false;
			if (rotate_only_) {
				wait_until_completion_ = false;
				target_x_ = current_x_;
				target_y_ = current_y_;
				target_angle_ = current_angle_;
			}
		} else {
			// Rotate the wheels in opposite directions so we rotate on the spot
			if (simulation_) {
				motor.left = -max_speed_ * heading_sign * hf;
				motor.right = max_speed_ * heading_sign * hf;
			} else {
				motor.left = max_speed_ * heading_sign * hf;
				motor.right = -max_speed_ * heading_sign * hf;
			}

			float max = std::max(std::abs(motor.left), std::abs(motor.right));
			if (max > 1.0f) {
				motor.left /= max;
				motor.right /= max;
			}

			float left_abs = std::abs(motor.left);
			float right_abs = std::abs(motor.right);
			if (left_abs < min_speed_) {
				motor.left = (motor.left < 0.0f ? -1.0f : 1.0f) * min_speed_;
			}
			if (right_abs < min_speed_) {
				motor.right = (motor.right < 0.0f ? -1.0f : 1.0f) * min_speed_;
			}
		}
	} else {
		if (target_distance < distance_accuracy_) {
			// Target reached, stop your engines!
			motor.left = motor.right = 0.0f;
			wait_until_completion_ = false;
		} else {
			// Go straight but keep adjusting for the heading 
			// being off.
			if (simulation_) {
				motor.left = max_speed_ * (df - (heading_sign * hf * 0.75f));
				motor.right = max_speed_ * (df + (heading_sign * hf * 0.75f));
			} else {
				motor.left = max_speed_ * (df + (heading_sign * hf * 0.25f));
				motor.right = max_speed_ * (df - (heading_sign * hf * 0.25f));
			}

			float max = std::max(std::abs(motor.left), std::abs(motor.right));
			if (max > 1.0f) {
				motor.left /= max;
				motor.right /= max;
			}

			float left_abs = std::abs(motor.left);
			float right_abs = std::abs(motor.right);
			if (left_abs < min_speed_) {
				motor.left = (motor.left < 0.0f ? -1.0f : 1.0f) * min_speed_;
			}
			if (right_abs < min_speed_) {
				motor.right = (motor.right < 0.0f ? -1.0f : 1.0f) * min_speed_;
			}
		}
	}

	mot_pub.publish(motor);
}

void Motor::updateOdometry(float diff_time, int diff_left_ticks, int diff_right_ticks)
{
	// Calculate the distance each wheel traveled based on the number of ticks
	float dist_left = distance_per_tick_ * diff_left_ticks;
	float dist_right = distance_per_tick_ * diff_right_ticks;

	// Calculate the new position of the robot given the distance each wheel traveled.
	// The calculations are best understood by looking at the graphics on page 2 in this PDF:
	// http://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-186-mobile-autonomous-systems-laboratory-january-iap-2005/study-materials/odomtutorial.pdf

	// Calculate distance the center of the robot traveled
	float dist_center = (dist_left + dist_right) / 2.0f;
	current_speed_ = dist_center / diff_time;

	// Calculate the change in position and angle
	float delta_theta = -(dist_right - dist_left) / wheel_base_;
	float delta_x = dist_center * cosf(current_angle_ + delta_theta);
	float delta_y = dist_center * sinf(current_angle_ + delta_theta);

	// Update the global position
	current_x_ += delta_x;
	current_y_ += delta_y;
	current_angle_ += delta_theta;

	// Publish movement
	publishMovement(dist_left, dist_right);
}

void Motor::publishMovement(float dist_left, float dist_right)
{
	// Publish the angle and distance traveled
	driver::Movement movement;
	movement.dist_left = dist_left;
	movement.dist_right = dist_right;
	mov_pub.publish(movement);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "driver");

	new Motor();

	return 0;
}
